/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package frc.circuitrunners.Archive;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;
/**
 *
 * @author James
 */
public class HolonomicDrive extends AbstractDrive {
    public HolonomicDrive(SpeedController[] mCs){
        if(mCs.length != 4){
            throw new IllegalArgumentException("Length of motor controllers array inccorect. Expected: 2 or 4. Given: " + mCs.length);
        }
        motorControllers = mCs;
    }
    public void drive(double x, double y, double z){
        final int fr = 0, fl = 1, br = 2, bl = 3;
        double[] motorspeeds = new double[4];
        motorspeeds[fr] = y + x + z;
        motorspeeds[fl] = y - x - z;
        motorspeeds[br] = y - x + z;
        motorspeeds[bl] = y + x - z;
        double highestValue = Math.abs(motorspeeds[0]);
        for(int i = 1; i < motorspeeds.length; i++){
            double temp = Math.abs(motorspeeds[i]);
            if(highestValue < temp){
                highestValue = temp;
            }
        }
        if(highestValue > 1.0){
            for(int i = 0; i < motorspeeds.length; i++){
                motorspeeds[i] = motorspeeds[i] / highestValue;
            }
        }
        motorControllers[FRONT_RIGHT].set(motorspeeds[fr]);
        motorControllers[FRONT_LEFT].set(motorspeeds[fl]);
        motorControllers[BACK_RIGHT].set(motorspeeds[br]);
        motorControllers[BACK_LEFT].set(motorspeeds[bl]);
    }
    
}
